#include "trajtest.h"
#include "../../robotattribute.h"
#include "../../mccontrollerinterpreter.h"
#include "../../mccontrollerthread.h"

TrajTest::TrajTest()
{
}

int TrajTest::test1()
{
    QVector<JointAttribute> robotAttribute;

    RobotAttribute robotA("mcControllerInterpreter.xml",1);

    RobotCommonParameter roboParameter;
    robotA.getRobotAttribute(roboParameter);

    Coordinate coordinateServer(roboParameter.robotType,roboParameter.internalNum,roboParameter.robotId,&roboParameter.jointAttributeVector  ) ;

    TrajectoryServer trajServer(&roboParameter,&coordinateServer);




    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;
    targetPoint.jointFlag=0;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=30;
    targetPoint.positionValue[1]=40;
    targetPoint.positionValue[2]=50;
    targetPoint.positionValue[3]=60;
    targetPoint.positionValue[4]=130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_JOINT;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 50;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);

    trajServer.addTrajectoryCommand(motionParaList);
    trajServer.calculateTrajectoryPath();

    trajServer.setTimeAxisScale(1,0.1);

    int returnValue;

    do
    {

        returnValue=trajServer.moveOneStep();


    }
    while (1==returnValue);

    trajServer.writeDataToFile();



}

int TrajTest::testMcInterpreter()
{




    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;
    targetPoint.jointFlag=0;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=30;
    targetPoint.positionValue[1]=40;
    targetPoint.positionValue[2]=50;
    targetPoint.positionValue[3]=60;
    targetPoint.positionValue[4]=130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_JOINT;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 50;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);


    targetPoint.jointFlag=0;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=-30;
    targetPoint.positionValue[1]=-40;
    targetPoint.positionValue[2]=-50;
    targetPoint.positionValue[3]=-60;
    targetPoint.positionValue[4]=-130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_JOINT;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 50;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);





    McControllerThread mcThread(1);
    McControllerInterpreter* mc;
    mc=mcThread.getControllerInterpreter();

    mc->addTrajectoryCommand(motionParaList);
    mc->startMove();
    mcThread.start();

    sleep(1);
    while(0==mc->isTrajectoryEmpty())
    {
        mc->algorithmFunctioncall();
    }

    mcThread.quit();

    mc->writeDataToFile();





    return 1;
}
